from launch import LaunchDescription
from launch_ros.actions import Node
from launch.substitutions import PathJoinSubstitution
from launch_ros.substitutions import FindPackageShare


def generate_launch_description():
    # 1. 找到配置文件路径（假设配置文件在 config/bridge_config.yaml）
    config_file_path = PathJoinSubstitution(
        [FindPackageShare("ros2_mqtt_bridge"), "config", "bridge_config.yaml"]
    )

    # 2. 定义 MQTT Bridge 节点
    mqtt_bridge_node = Node(
        package="ros2_mqtt_bridge",  # 功能包名
        executable="mqtt_bridge_node",  # 节点可执行文件名（需与 CMakeLists.txt 中一致）
        name="mqtt_bridge_node",  # 节点名称（可自定义，避免冲突）
        output="screen",  # 日志输出到终端
        parameters=[
            config_file_path  # 加载 YAML 配置文件
        ],
        # 可选：如果需要重写配置参数，可在此处添加（优先级高于 YAML）
        # arguments=[
        #     "--ros-args",
        #     "-p", "mqtt.host:=192.168.1.100",  # 重写 MQTT 服务器地址
        #     "-p", "ros2_to_mqtt.topic_ros:=/custom_ros_topic"  # 重写 ROS 发送主题
        # ]
    )

    # 3. 组装 Launch 描述
    return LaunchDescription([
        mqtt_bridge_node
    ])